import numpy as np
from PIL import Image

color_image_name = 'color.jpg'
mapping_matrix = np.array([[2.83269438e+00, -5.68796925e-03, 1.73472348e-18],
                           [-9.28598668e-03, 2.94509748e+00, -1.73472348e-18],
                           [3.33378047e+02, -9.36243388e+01, 1.00000000e+00]])
# mapping_matrix = np.array([[1.7705, -0.2490, 0.0000], [-1.2077, 2.8351, 0.0000], [950.3224, 4.7978,
# 1.0000]]) mapping_matrix = np.linalg.inv(np.array([[0.3584, 0.0089, 0.0000], [0.0031, 0.3531, 0.0001], [-101.5934,
# 13.6311, 0.9914]])) mapping_matrix = np.array([[0.3584, 0.0089, 0.0000], [0.0031, 0.3531, 0.0001], [-101.5934,
# 13.6311, 0.9914]]) mapping_matrix = np.array([[0.9998, 0.00062, -0.0207], [-0.0063, 1.0000, -0.0059], [0.0206,
# 0.0061, 0.9998]])
print(mapping_matrix)
color_image = np.array(Image.open(color_image_name))
print(color_image.shape)
mapped_color_image = np.zeros((424, 512, 3), dtype='uint8')
print(np.dot(np.array([277, 228, 1]), mapping_matrix))
for i in range(424):
    for j in range(512):
        coordinate = np.dot(np.array([j, i, 1]), mapping_matrix)
        # print(coordinate.shape)
        color_x = coordinate[0]
        color_y = coordinate[1]
        if not (color_x == '-inf' or color_y == '-inf'):
            color_x = int(round(float(color_x)))
            color_y = int(round(float(color_y)))
            if 0 <= color_x < 1920 and 0 <= color_y < 1080:
                mapped_color_image[i][j][0] = int(color_image[color_y][color_x][0])
                mapped_color_image[i][j][1] = int(color_image[color_y][color_x][1])
                mapped_color_image[i][j][2] = int(color_image[color_y][color_x][2])
# for i in range(1080):
#     for j in range(1920):
#         coordinate = np.dot(mapping_matrix, np.array([i, j, 1]))
#         # print(coordinate.shape)
#         color_x = coordinate[1]
#         color_y = coordinate[0]
#         if not (color_x == '-inf' or color_y == '-inf'):
#             color_x = int(round(float(color_x)))
#             color_y = int(round(float(color_y)))
#             if 0 <= color_x < 512 and 0 <= color_y < 424:
#                 mapped_color_image[color_y][color_x][0] = int(color_image[i][j][0])
#                 mapped_color_image[color_y][color_x][1] = int(color_image[i][j][1])
#                 mapped_color_image[color_y][color_x][2] = int(color_image[i][j][2])

img = Image.fromarray(mapped_color_image)
img.save('test.jpg')
